package edu.archwood.frc2607.logomotion;
import edu.archwood.frc2607.utility.ConsoleLogger;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
public class LogomotionTelescopingArm implements LogomotionConstants{
    AnalogChannel Potentiometer;
    CANJaguar ShoulderJag = null, SuckerRelay = null;
//    DigitalInput ExtendSwitch;
//    DigitalInput RetractSwitch;
    Solenoid theElbow;
    private ConsoleLogger cl = ConsoleLogger.getInstance();

//    Solenoid Claw;
    LogomotionTelescopingArm()
    {
        while (ShoulderJag == null) {
            try {
                ShoulderJag = new CANJaguar(8);
                // TelescopeRelay = new CANJaguar(14);
                SuckerRelay = new CANJaguar(14);
                cl.log("ShoulderJag created successfully");
            } catch (Exception e) {
                cl.log("ShoulderJag exception, retrying...");
                Timer.delay(.25);
            }
        }

          theElbow = new Solenoid(1);
  //      ExtendSwitch = new DigitalInput(9);
  //      RetractSwitch = new DigitalInput(8);
        Potentiometer = new AnalogChannel(7);
    }
    public double getPot()
    {
        double returnvalue = Potentiometer.getAverageValue();
        return (returnvalue);
    }
    public void rotateShoulder(double speed)
    {
        //if (getPot() <= shoulderMinPosition || getPot() >= shoulderMaxPosition)
          //  speed = 0;
        try {
            ShoulderJag.setX(speed);
        } catch (Exception e) {
            cl.log("ShoulderJag exception setting speed");
        }
    }

    public boolean gotoPosition(double shoulder) {
        // return true when at position, false when moving to position
        double shoulderPos = Potentiometer.getAverageValue();


        // NOTE:  Pot gain is the opposite of how it is on LogomotionArm; below
        // updated to reflect that

        // FOR THE TELESCOPING ARM:
        // shoulderPos decreases as shoulder lowers (speed < 0)
        // shoulderPos increases as shoulder raises (speed > 0)
        // if desired shoulder position < current position, lower shoulder
        // if desired shoulder position > current position, raise shoulder
        // err = shoulderPos - shoulder, sign is opposite to motor direction
        double shoulderErr = shoulderPos - shoulder;
        double shoulderSpeed = 0.0;
        boolean done = true;
        if (Math.abs(shoulderErr) > 10) {
            if (shoulderErr < 0) shoulderSpeed = -.5;
            else shoulderSpeed = .85;
            done = false;
        }
        rotateShoulder(shoulderSpeed);
        return done;
    }

    public String getPotValues() {
        String s = Integer.toString(Potentiometer.getAverageValue());
        if (s.length() > 3) s = s.substring(0,3);
        return "shld: " + s + "   ";
    }

    public void suckOrSpitTube(byte todo) throws CANTimeoutException
    {
        switch (todo)
        {
            case 1:
                SuckerRelay.setX(1);
            break;
            case 0:
                SuckerRelay.setX(0);
            break;
            case -1:
                SuckerRelay.setX(-1);
            break;
        }
    }
    

  /*  public void telescopeArm(byte speed) throws CANTimeoutException
    {
            switch (speed)
            {
                case 1:
                  if (getExtended() != 2)
                    {
                    TelescopeRelay.setX(1);
                    }
                break;
                case 0:
                    TelescopeRelay.setX(0);
                break;
                case -1:
                    if (getExtended() != 0)
                    {
                    TelescopeRelay.setX(-1);
                    }
                break;
            }
    } */
     /**
     * @return Returns -1 in error, 0 when retracted, 1 when neither retracted nor extended, or 2 when extended
     */
  /*  public String getSwitches()
    {
        return ("Ext13:"+ExtendSwitch.get()+", Ret14:"+RetractSwitch.get());
    }
    public short getExtended()
    {
        short returnvalue = -1;
        if (!ExtendSwitch.get())
        {
            returnvalue = 2;
        }
        else
        {
            if (!RetractSwitch.get())
            {
                returnvalue = 0;
            }
            else {
                returnvalue = 1;
            }
        }
        return (returnvalue);
    }  */
    public String getTelemetryHeader() {
        return "ShoulderPot,Extended State,Shoulder Speed,Telescpe Speed";
    }

    public String getTelemetryData() {
        String returnvalue = "N/A,N/A,N/A,N/A";
        return returnvalue;
    }
 public double getSpeed() {
        try {
            return ShoulderJag.getSpeed();
        }
        catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        return 0;
    }
    public void useTheElbow(boolean position) {
        theElbow.set(position);
    }
}